#include <rclcpp/rclcpp.hpp>

#include "node/node.h"

AppNode::AppNode(const string &node_name) : Node(node_name) {
    timer_ = this->create_wall_timer(chrono::seconds(1), bind(&AppNode::timer_callback, this));
    RCLCPP_INFO(this->get_logger(), "App node initialized");
}

// AppNode::~AppNode() { RCLCPP_INFO(this->get_logger(), "App destroyed"); }

void AppNode::timer_callback() {
    // RCLCPP_INFO(this->get_logger(), "Timer callback executed");
}

bool AppNode::setParameter(const string &key, const string &value) {
    // Create parameter client
    auto client = this->create_client<SetParas>("/mbot_body/set_parameter");
    // 此处wait_for_service会阻塞，直到服务可用
    // 也可以使用rclcpp::spin_until_future_complete()来等待服务可用
    // 但是需要注意spin_until_future_complete()会阻塞当前线程
    // 可能会导致其他回调函数无法执行
    if (!client->wait_for_service(chrono::seconds(1))) {
        if (!rclcpp::ok()) {
            RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
            return false;
        }
        RCLCPP_INFO(this->get_logger(), "Service not available, please try again...");
        return false;
    }

    auto request = std::make_shared<SetParas::Request>();
    rcl_interfaces::msg::Parameter parameter;
    parameter.name = key;
    parameter.value.string_value = value;
    parameter.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
    request->parameters.push_back(parameter);

    auto future = client->async_send_request(request);
    if (rclcpp::spin_until_future_complete(this->shared_from_this(), future) != rclcpp::FutureReturnCode::SUCCESS) {
        RCLCPP_ERROR(this->get_logger(), "Failed to call service");
        return false;
    }

    auto response = future.get();
    // Only one parameter is set, so we can check the first result
    // bool all_successful = std::all_of(response->results.begin(), response->results.end(),
    //                                   [](const auto &result) { return result.successful; });
    if (response->results[0].successful) {
        RCLCPP_INFO(this->get_logger(), "Parameter set successfully");
    } else {
        RCLCPP_ERROR(this->get_logger(), "Failed to set parameter");
        return false;
    }

    return true;
}

const string AppNode::getParameter(const string &key) {
    // Create parameter client
    auto client = this->create_client<GetParas>("/mbot_body/get_parameter");
    if (!client->wait_for_service(chrono::seconds(1))) {
        if (!rclcpp::ok()) {
            RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
            return "";
        }
        RCLCPP_INFO(this->get_logger(), "Service not available, please try again...");
        return "";
    }
    auto request = std::make_shared<GetParas::Request>();
    rcl_interfaces::msg::Parameter parameter;
    parameter.name = key;
    parameter.value.string_value = "";
    parameter.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
    request->names.push_back(parameter.name);
    auto future = client->async_send_request(request);
    if (rclcpp::spin_until_future_complete(this->shared_from_this(), future) != rclcpp::FutureReturnCode::SUCCESS) {
        RCLCPP_ERROR(this->get_logger(), "Failed to call service");
        return "";
    }
    auto response = future.get();
    if (response->values.size() == 0) {
        RCLCPP_ERROR(this->get_logger(), "Failed to get parameter");
        return "";
    }
    if (response->values[0].type == rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
        return response->values[0].string_value;
    } else {
        RCLCPP_ERROR(this->get_logger(), "Parameter type mismatch");
        return "";
    }
}

// 获取所有参数的当前值
const map<string, string> AppNode::getAllParameters() {
    // Create parameter client
    auto client = this->create_client<GetParas>("/mbot_body/get_all_parameters");
    if (!client->wait_for_service(chrono::seconds(1))) {
        if (!rclcpp::ok()) {
            RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
            return {};
        }
        RCLCPP_INFO(this->get_logger(), "Service not available, please try again...");
        return {};
    }
    auto request = std::make_shared<GetParas::Request>();
    auto future = client->async_send_request(request);
    if (rclcpp::spin_until_future_complete(this->shared_from_this(), future) != rclcpp::FutureReturnCode::SUCCESS) {
        RCLCPP_ERROR(this->get_logger(), "Failed to call service");
        return {};
    }
    auto response = future.get();
    if (response->values.size() == 0) {
        RCLCPP_ERROR(this->get_logger(), "Failed to get parameters");
        return {};
    }
    map<string, string> parameters;
    for (const auto &value : response->values) {
        // response format: key:value
        if (value.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) {
            RCLCPP_ERROR(this->get_logger(), "Parameter type mismatch");
            return {};
        }
        std::string str_value = value.string_value;
        size_t delimiter_pos = str_value.find(':');
        if (delimiter_pos == std::string::npos) {
            RCLCPP_ERROR(this->get_logger(), "Invalid parameter format");
            return {};
        }
        std::string key = str_value.substr(0, delimiter_pos);
        std::string val = str_value.substr(delimiter_pos + 1);
        parameters[key] = val;
    }
    return parameters;
}

// 获取所有参数的全部信息，用于App中的参数初始化填充
// 对于列表参数，返回值中包含参数名、所有有效参数值
// 对于单值参数，返回值中仅包含参数名
const map<string, vector<string>> AppNode::listParameters() {
    // Create parameter client
    auto client = this->create_client<rcl_interfaces::srv::ListParameters>("/mbot_body/list_parameters");
    if (!client->wait_for_service(chrono::seconds(1))) {
        if (!rclcpp::ok()) {
            RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
            return {};
        }
        RCLCPP_INFO(this->get_logger(), "Service not available, please try again...");
        return {};
    }
    auto request = std::make_shared<rcl_interfaces::srv::ListParameters::Request>();
    request->depth = 1;
    auto future = client->async_send_request(request);
    if (rclcpp::spin_until_future_complete(this->shared_from_this(), future) != rclcpp::FutureReturnCode::SUCCESS) {
        RCLCPP_ERROR(this->get_logger(), "Failed to call service");
        return {};
    }
    auto response = future.get();
    if (response->result.names.size() == 0) {
        RCLCPP_ERROR(this->get_logger(), "Failed to list parameters");
        return {};
    }
    map<string, vector<string>> parameters;
    for (const auto &name : response->result.names) {
        // response format: key:value1,value2,value3
        size_t delimiter_pos = name.find(':');
        if (delimiter_pos == std::string::npos) {
            RCLCPP_ERROR(this->get_logger(), "Invalid parameter format");
            return {};
        }
        std::string key = name.substr(0, delimiter_pos);
        std::string val = name.substr(delimiter_pos + 1);

        // Split the value by comma
        std::string delimiter = ",";
        size_t pos = 0;
        std::string token;
        while ((pos = val.find(delimiter)) != std::string::npos) {
            token = val.substr(0, pos);
            parameters[key].push_back(token);
            val.erase(0, pos + delimiter.length());
        }
    }
    return parameters;
}
